#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import sys, rospy, time
from std_msgs.msg import Float64
from std_msgs.msg import String


class General():
    def __init__(self):
        self.pub_navigation = rospy.Publisher('start_navigation', String, queue_size=10)
        self.pub_listen = rospy.Publisher('voiceWakeup', String, queue_size=10)
        self.pub_voice = rospy.Publisher('voice', String, queue_size=10)
        self.pub_pid = rospy.Publisher('pid', String, queue_size=10)
        self.pub_arm = rospy.Publisher('arm', String, queue_size=10)
        self.pub_vision = rospy.Publisher('vision', String, queue_size=10)
        self.pub_qr = rospy.Publisher('qr', String, queue_size=10)
        self.navigation=0
        
        
        
          

    def listen(self):
        # 1.开始语音，询问购物需求
        print('Task Begin')
        #需要手动发启动消息
        #self.pub_voice.publish('begin')
        #self.pub_voice.publish('begin')
        rospy.Subscriber("arm_back", String, self.arm_back)
        rospy.Subscriber("navigation_back", String, self.navigation_back)
        rospy.Subscriber("voice_back", String, self.voice_back)
        rospy.Subscriber("PID_back", String, self.pid_back)
        rospy.Subscriber("qr_back", String, self.qr_back)

    def voice_back(self, data):
        print(data)
        sound = data.data
        print(sound)
        # 2.得知购物需求，发送到视觉，导航前往购物
        if "cup" in sound or "orange" in sound or "book" in sound:
            print('get_requested_item')
            msg = "shopping"
            self.pub_navigation.publish(msg)
            if "cup" in sound:
                item = "cup"
            elif "Cup" in sound:
                item = "cup"
            elif "orange" in sound:
                item = "orange"
            elif "Orange" in sound:
                item = "orange"
            elif "book" in sound:
                item = "book"
            elif "Book" in sound:
                item = "book"
            self.pub_vision.publish(item) # 视觉待识别物体
            self.pub_voice.publish(item) # 语音重复需求
        # 7.价格播报完毕，导航返回
        if "pay_finish" in sound:
            print('pay_finish')
            msg = "back"

            self.pub_navigation.publish(msg)

    def navigation_back(self, data):
		# 3.到达购物区，开始pid
        if data.data == "navigation1_finish":
            print('navigation1_finish')
            self.pub_pid.publish('adjust')
        # 8.回到起点，机械臂给出去
        if data.data == "navigation2_finish":
            print('navigation2_finish')
            self.pub_arm.publish('give')

    def pid_back(self, data):
        # 4.pid调整完毕，开始抓取
        if data.data == "adjust_finish":
            print('adjust_finish')
            self.pub_arm.publish("grasp")

    def arm_back(self, data):
        # 5.抓取完毕，开始支付
        if data.data == "grasp_finish":
            print('grasp_finish')
            msg="scan"
            self.pub_qr.publish(msg)
        # 9.传递完毕，任务结束
        if data.data == "hand_finish":
            msg="all_over"
            self.pub_voice.publish(msg)
            print("hand_finish")
            
    def qr_back(self, data):
        # 6.支付完毕，播报价格
        price = data.data
        if "yuan" in price:
            self.pub_voice.publish(price)


if __name__ == '__main__':
    rospy.init_node('main', anonymous=True)
    P = General()
    P.listen()
    rospy.spin()
